
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.Vec3;

import { Vec3 } from "../../../../common/Vec3";
import { M } from "../../../../common/M";

/**
 * Simplex utilities for GJK/EPA computations.
 */

export class SimplexUtil {
	/**
	 * Sets `out` to the minimum length point on the line (`vec1`, `vec2`)
	 * and returns the index of the voronoi region.
	 */
	public static projectOrigin2(vec1: Vec3, vec2: Vec3, out: Vec3): number {
		var v1: Vec3 = vec1;
		var v2: Vec3 = vec2;

		let v12: Vec3 = v2.sub(v1);

		var d: number = v12.dot(v12);
		var t: number = v12.dot(v1);
		t = -t / d;

		if (t < 0) {
			out.copyFrom(v1);
			return 1;
		}
		if (t > 1) {
			out.copyFrom(v2);
			return 2;
		}

		// dst = src1 op (src2 op2 src3)
		// out = v1 add (v12 * t);
		v1.addScaledTo(v12, t, out);
		return 3;
	}

	/**
	 * Sets `out` to the minimum length point on the triangle (`vec1`, `vec2`, `vec3`)
	 * and returns the index of the voronoi region.
	 */
	public static projectOrigin3(vec1: Vec3, vec2: Vec3, vec3: Vec3, out: Vec3): number {
		let v1: Vec3 = vec1;
		let v2: Vec3 = vec2;
		let v3: Vec3 = vec3;
		let v12: Vec3 = v2.sub(v1);
		let v23: Vec3 = v3.sub(v2);
		let v31: Vec3 = v1.sub(v3);

		let n: Vec3 = v12.cross(v23);

		let n12: Vec3 = v12.crossEq(n);
		let n23: Vec3 = v23.crossEq(n);
		let n31: Vec3 = v31.crossEq(n);

		let d12: number = v1.dot(n12);
		let d23: number = v2.dot(n23);
		let d31: number = v3.dot(n31);

		var mind: number = -1;
		var minv: Vec3 = new Vec3();
		var mini: number = 0; // index of voronoi region

		if (d12 < 0) {
			var b: number = SimplexUtil.projectOrigin2(vec1, vec2, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			mini = b;
			mind = d;
			minv.copyFrom(out);
		}
		if (d23 < 0) {
			var b: number = SimplexUtil.projectOrigin2(vec2, vec3, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			if (mind < 0 || d < mind) {
				mini = b << 1; // 00000021 -> 00000210
				mind = d;
				minv.copyFrom(out);
			}
		}
		if (d31 < 0) {
			var b: number = SimplexUtil.projectOrigin2(vec1, vec3, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			if (mind < 0 || d < mind) {
				mini = b & 1 | (b & 2) << 1; // 00000021 -> 00000201
				mind = d;
				minv.copyFrom(out);
			}
		}
		if (mind > 0) {
			out.copyFrom(minv);
			return mini;
		}

		n.normalize();
		let dn: number = v1.dot(n);
		let l2: number = n.dot(n);
		l2 = dn / l2;
		n.scaleTo(l2, minv);
		out.copyFrom(minv);
		return 7;
	}

	/**
	 * Sets `out` to the minimum length point on the tetrahedron (`vec1`, `vec2`, `vec3`, `vec4`)
	 * and returns the index of the voronoi region.
	 */
	public static projectOrigin4(vec1: Vec3, vec2: Vec3, vec3: Vec3, vec4: Vec3, out: Vec3): number {
		let v1: Vec3 = vec1;
		let v2: Vec3 = vec2;
		let v3: Vec3 = vec3;
		let v4: Vec3 = vec4;
		let v12: Vec3 = v2.sub(v1);
		let v13: Vec3 = v3.sub(v1);;
		let v14: Vec3 = v4.sub(v1);
		let v23: Vec3 = v2.sub(v3);;
		let v24: Vec3 = v4.sub(v2);
		let v34: Vec3 = v4.sub(v4);

		let rev: boolean = false;
		let n123: Vec3 = v12.cross(v13);
		let n134: Vec3 = v13.crossEq(v14);
		let n142: Vec3 = v14.crossEq(v12);
		let n243: Vec3 = v24.crossEq(v23);
		let n: Vec3;

		var sign: number = v12.dot(n243) > 0 ? 1 : -1;
		var d123: number = M.vec3_dot(v1, n123);
		var d134: number = M.vec3_dot(v1, n134);
		var d142: number = M.vec3_dot(v1, n142);
		var d243: number = M.vec3_dot(v2, n243);

		var mind: number = -1;
		var minv: Vec3 = new Vec3();
		var mini: number = 0; // index of voronoi region

		if (d123 * sign < 0) {
			var b: number = SimplexUtil.projectOrigin3(vec1, vec2, vec3, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			mini = b;
			mind = d;
			minv.copyFrom(out);
		}
		if (d134 * sign < 0) {
			var b: number = SimplexUtil.projectOrigin3(vec1, vec3, vec4, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			if (mind < 0 || d < mind) {
				mini = b & 1 | (b & 6) << 1; // 00000321 -> 00003201
				mind = d;
				minv.copyFrom(out);
			}
		}
		if (d142 * sign < 0) {
			var b: number = SimplexUtil.projectOrigin3(vec1, vec2, vec4, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			if (mind < 0 || d < mind) {
				mini = b & 3 | (b & 4) << 1; // 00000321 -> 00003021
				mind = d;
				minv.copyFrom(out);
			}
		}
		if (d243 * sign < 0) {
			var b: number = SimplexUtil.projectOrigin3(vec2, vec3, vec4, out);
			var d: number = out.x * out.x + out.y * out.y + out.z * out.z;
			if (mind < 0 || d < mind) {
				mini = b << 1; // 00000321 -> 00003210
				mind = d;
				minv.copyFrom(out);
			}
		}

		if (mind > 0) {
			out.copyFrom(minv);
			return mini;
		}

		// the origin is inside the tetrahedron
		out.zero();
		return 15;
	}
}
